#include "rclcpp/rclcpp.hpp"
#include <behaviortree_cpp_v3/bt_factory.h>
#include <behaviortree_cpp_v3/utils/shared_library.h>
#include <behaviortree_cpp_v3/control_node.h>
#include <behaviortree_cpp_v3/action_node.h>
#include <behaviortree_cpp_v3/condition_node.h>


#include <GameStart.hpp>

#include <pubRobotStatus.hpp>
#include <Status.hpp>
#include <filesystem>   
#include <ament_index_cpp/get_package_share_directory.hpp>
int main(int argc, char* argv[])
{
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("bt_node"); 
  // std::string path = "/home/rm/slam/ws/src/hnu-vision-ros/hnurm_decision/src/decision.xml";
  std::string base_dir = ament_index_cpp::get_package_share_directory("hnurm_decision");

  std::string path = base_dir + "/params/testing.xml";


  BT::BehaviorTreeFactory factory;
  BT::SharedLibrary loader;

  factory.registerNodeType<hnurm_behavior_trees::GameStart>("GameStart");

  factory.registerNodeType<hnurm_behavior_trees::pubRobotStatus>("pubRobotStatus");
  factory.registerNodeType<hnurm_behavior_trees::Status>("Status");
  
  factory.registerFromPlugin(loader.getOSName("nav2_is_stuck_condition_bt_node"));
  factory.registerFromPlugin(loader.getOSName("nav2_navigate_to_pose_action_bt_node"));
  factory.registerFromPlugin(loader.getOSName("nav2_wait_action_bt_node"));
  factory.registerFromPlugin(loader.getOSName("nav2_wait_cancel_bt_node"));
  factory.registerFromPlugin(loader.getOSName("nav2_goal_reached_condition_bt_node"));
  factory.registerFromPlugin(loader.getOSName("nav2_single_trigger_bt_node"));
  factory.registerFromPlugin(loader.getOSName("nav2_pipeline_sequence_bt_node"));
  factory.registerFromPlugin(loader.getOSName("nav2_rate_controller_bt_node"));
  factory.registerFromPlugin(loader.getOSName("nav2_follow_path_action_bt_node"));
  factory.registerFromPlugin(loader.getOSName("nav2_truncate_path_action_bt_node"));
  factory.registerFromPlugin(loader.getOSName("nav2_goal_updater_node_bt_node"));
  factory.registerFromPlugin(loader.getOSName("nav2_compute_path_to_pose_action_bt_node"));
  factory.registerFromPlugin(loader.getOSName("nav2_clear_costmap_service_bt_node"));
  factory.registerFromPlugin(loader.getOSName("nav2_drive_on_heading_bt_node"));
  factory.registerFromPlugin(loader.getOSName("nav2_goal_updated_condition_bt_node"));



  BT::Blackboard::Ptr blackboard = BT::Blackboard::create();
  blackboard->set("node",node);
  blackboard->set("server_timeout", std::chrono::milliseconds (100000));
  blackboard->set("bt_loop_duration",std::chrono::milliseconds(100));
  blackboard->set("wait_for_service_timeout",  std::chrono::milliseconds (100000)); // 写入 Blackboard
  auto tree = factory.createTreeFromFile(path,blackboard);

  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(node);
 // 以一定频率持续运行行为树
    rclcpp::Rate loop_rate(50);  // 50 Hz
    while (rclcpp::ok()) {
        tree.tickRoot();
        // rclcpp::spin_some(node);
        executor.spin_some();
        loop_rate.sleep();
    }

  // tree.tickR
  rclcpp::shutdown();
  return 0;
}
